/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief  服务模块的主运行逻辑模块实现文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/layered_costmap.h>

#include <algorithm>
#include <cstdio>
#include <iostream>
#include <string>
#include <vector>

using namespace std;

namespace costmap_2d {

Costmap2DROS::Costmap2DROS(const std::string& name, std::string config,
                           DataCollector* collector)
    : layered_costmap_(NULL),
      name_(name),
      map_update_thread_shutdown_(false),
      stop_updates_(false),
      robot_stopped_(false),
      map_update_thread_(NULL),
      movement_check_thread_(NULL),
      last_publish_(0),
      publisher_(NULL),
      footprint_padding_(0.0) {
  // we need to make sure that the transform between the robot base frame and
  // the global frame is available 读取参数和获取数据类
  configNode_ = new ConfigFile(config);
  configNode_->ExtractKeys();
  dataCollector_ = collector;

  // 确认所需的数据都已经收到了
  while (true) {
    costmap_2d::PointCloud pts;
    costmap_2d::OccupancyGrid map;
    costmap_2d::Pose odom;

    if (!dataCollector_->getPts(pts)) {
      std::cout << "Keep Waiting For PointCloud" << std::endl;
    }
    if (!dataCollector_->getMap(map)) {
      std::cout << "Keep Waiting For Map" << std::endl;
    }
    if (!dataCollector_->getOdom(odom)) {
      std::cout << "Keep Waiting For Map" << std::endl;
    }
    if (dataCollector_->getPts(pts) && dataCollector_->getMap(map) &&
        dataCollector_->getOdom(odom)) {
      std::cout << "=======================" << std::endl;
      std::cout << "Already get the msg we need !!!" << std::endl;
      std::cout << "=======================" << std::endl;
      break;
    }
    sleep(0.5);
  }
  dataCollector_->getOdom(old_pose_);

  // check if we want a rolling window version of the costmap
  bool rolling_window, track_unknown_space, always_send_full_costmap;

  rolling_window = (*configNode_).getValueOfKey<bool>("rolling_window", false);
  track_unknown_space =
      (*configNode_).getValueOfKey<bool>("track_unknown_space", false);

  double publish_cycle =
      (*configNode_).getValueOfKey<double>("publish_map_frequency", 0.2);

  layered_costmap_ =
      new LayeredCostmap("map", rolling_window, track_unknown_space);

  // 开始加载各个Layer。
  bool using_static_layer, using_obstacle_layer, using_inflation_layer;

  using_static_layer =
      (*configNode_).getValueOfKey<bool>("using_static_layer", false);
  using_obstacle_layer =
      (*configNode_).getValueOfKey<bool>("using_obstacle_layer", false);
  using_inflation_layer =
      (*configNode_).getValueOfKey<bool>("using_inflation_layer", false);

  //  改plugin写法为虚拟接口,这里将layered_costmap参数传入到各个plugin中去
  if (using_static_layer) {
    layers_.push_back(
        boost::shared_ptr<costmap_2d::Layer>(new costmap_2d::StaticLayer()));
  }
  if (using_obstacle_layer) {
    layers_.push_back(
        boost::shared_ptr<costmap_2d::Layer>(new costmap_2d::ObstacleLayer()));
  }
  if (using_inflation_layer) {
    layers_.push_back(
        boost::shared_ptr<costmap_2d::Layer>(new costmap_2d::InflationLayer()));
  }

  for (int i = 0; i < layers_.size(); i++) {
    layers_.at(i)->initialize(layered_costmap_, name_, configNode_,
                              dataCollector_);
    layered_costmap_->addPlugin(layers_.at(i));
  }

  // 设置footprint 以及 footprint padding
  footprint_padding_ =
      (*configNode_).getValueOfKey<double>("footprint_padding", 0.0);
  if ((*configNode_).getValueOfKeyAsString("footprint_type", "circle") ==
      "circle") {
    double radius;
    radius = (*configNode_).getValueOfKey<double>("radius", 0.0);
    setUnpaddedRobotFootprint(makeFootprintFromRadius(radius));
  } else if ((*configNode_).getValueOfKeyAsString("footprint_type", "circle") ==
             "polygon") {
    std::vector<std::vector<double> > polygon;
    std::vector<double> temp_polygon;
    temp_polygon =
        (*configNode_).getValueOfKeyAsStdVectorDouble("polygon_points", "");
    for (int i = 0; i < temp_polygon.size() / 2; i++) {
      std::vector<double> temp_data;
      temp_data.push_back(temp_polygon.at(2 * i));
      temp_data.push_back(temp_polygon.at(2 * i + 1));
      polygon.push_back(temp_data);
    }
    setUnpaddedRobotFootprint(toPointVector(polygon));
  }
  publisher_ = new Costmap2DPublisher(name_, layered_costmap_->getCostmap(),
                                      configNode_);

  // 设置总地图参数
  double map_width_meters = (*configNode_).getValueOfKey<double>("width", 0.0);
  double map_height_meters =
      (*configNode_).getValueOfKey<double>("height", 0.0);
  double resolution = (*configNode_).getValueOfKey<double>("resolution", 0.0);
  double origin_x = (*configNode_).getValueOfKey<double>("origin_x", 0.0);
  double origin_y = (*configNode_).getValueOfKey<double>("origin_y", 0.0);

  if (true) {
    layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
                                (unsigned int)(map_height_meters / resolution),
                                resolution, origin_x, origin_y);
  }

  // 创建处理map的线程
  double map_update_frequency =
      (*configNode_).getValueOfKey<double>("map_update_frequency", 0.2);
  map_update_thread_ = new boost::thread(
      boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
  map_update_thread_->detach();

  stop_updates_ = false;

  // 创建判断机器人是否移动的线程
  robot_stopped_ = false;
  movement_check_thread_ =
      new boost::thread(boost::bind(&Costmap2DROS::movementCheckLoop, this));
  movement_check_thread_->detach();
}

Costmap2DROS::~Costmap2DROS() {
  map_update_thread_shutdown_ = true;
  if (map_update_thread_ != NULL) {
    map_update_thread_->join();
    delete map_update_thread_;
  }
  if (movement_check_thread_ != NULL) {
    movement_check_thread_->join();
    delete movement_check_thread_;
  }
  if (publisher_ != NULL) {delete publisher_;}

  delete layered_costmap_;
}

void Costmap2DROS::setUnpaddedRobotFootprint(
    const std::vector<costmap_2d::Point>& points) {
  unpadded_footprint_ = points;
  padded_footprint_ = points;
  padFootprint(padded_footprint_, footprint_padding_);

  layered_costmap_->setFootprint(padded_footprint_);
}

void Costmap2DROS::movementCheckLoop() {
  while (true) {
    costmap_2d::Pose new_pose;
    if (!dataCollector_->getOdom(new_pose)) {
      robot_stopped_ = false;
    } else {
      old_pose_ = new_pose;

      robot_stopped_ =
          ((distance(new_pose.position.x, new_pose.position.y,
                     old_pose_.position.x, old_pose_.position.y) < 1e-3) &&
           (std::abs(getYaw(new_pose.orientation) -
                     getYaw(old_pose_.orientation))) < 1e-3);
    }
  }
}

void Costmap2DROS::mapUpdateLoop(double frequency) {
  // the user might not want to run the loop every cycle
  if (frequency == 0.0) {return;}
  while (!map_update_thread_shutdown_) {
    updateMap();
    if (publish_cycle > 0 && layered_costmap_->isInitialized()) {
      unsigned int x0, y0, xn, yn;
      layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
      publisher_->updateBounds(x0, xn, y0, yn);

      double now = time(0);
      if (last_publish_ + publish_cycle < now) {
        publisher_->publishCostmap();
        last_publish_ = now;
      }
    }
    sleep(frequency);
    // make sure to sleep for the remainder of our cycle time
  }
}

void Costmap2DROS::updateMap() {
  if (!stop_updates_) {
    // get global pose
    costmap_2d::Pose pose;
    if (dataCollector_->getOdom(pose)) {
      double x = pose.position.x, y = pose.position.y,
             yaw = getYaw(pose.orientation);
      layered_costmap_->updateMap(x, y, yaw);
      publisher_->publishFootprint(x, y, yaw, padded_footprint_);
    }
  }
}

void Costmap2DROS::start() { stop_updates_ = false; }

void Costmap2DROS::stop() { stop_updates_ = true; }

void Costmap2DROS::resetLayers() {
  Costmap2D* top = layered_costmap_->getCostmap();
  top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
}

}  // namespace costmap_2d
